import copy
from abc import ABCMeta, abstractmethod

import numpy as np

import util

N = 3


class SearchProblem(metaclass=ABCMeta):
    @abstractmethod
    def get_start_state(self):
        pass

    @abstractmethod
    def is_goal_state(self, state):
        pass

    @abstractmethod
    def get_successor(self, state):
        # return (next_state, action, cost)
        pass

    @abstractmethod
    def get_costs(self, actions):
        pass

    @abstractmethod
    def get_goal_state(self):
        pass


class Node:
    def __init__(self, state, path=[], priority=0):
        self.state = state
        self.path = path
        self.priority = priority

    def __le__(self, other):
        return self.priority <= other.priority

    def __lt__(self, other):
        return self.priority < other.priority


def search(problem, fringe, calc_heuristic=None, heuristic=None):
    """
    This is an simple abstracted graph search algorithm. You could
    using different combination of fringe storage, calc_heuristic, heuristic
    to implement different search algorithm.

    For example:
    LIFO Queue(Stack), None, None -> Depth First Search
    FIFO Queue, None, None -> Breath First Search
    PriorityQueue, cost compute function, None -> Uniform Cost Search

    In order to avoid infinite graph/tree problem we setup a list (visited) to
    avoid expanding the same node.

    hint: please check the node first before expanding:

    if node.state not in visited:
        visited.append(node.state)
    else:
        continue

    hint: you could get the successor by problem.get_successor method.

    hint: for fringe you may want to use
        fringe.pop  get a node from the fringe
        fringe.push   put a node into the fringe
        fringe.empty  check whether a fringe is empty or not. If the fringe is empty this function return True
        problem.is_goal_state check whether a state is the goal state
        problem.get_successor get all successor from current state
            return value: [(next_state, action, cost)]
    """
    start_state = problem.get_start_state()
    if isinstance(fringe, util.Stack) or isinstance(fringe, util.Queue):
        fringe.push(Node(start_state))
    else:
        fringe.push(Node(start_state), 0)
    visited = []
    step = 0
    while not fringe.empty():
        "*** YOUR CODE HERE ***"
        cur_node = fringe.pop()
        step += 1
        if not problem.is_goal_state(cur_node.state):
            if cur_node.state not in visited:  # 在expand之前判断是否visit过
                visited.append(cur_node.state)
                for next_tuple in problem.get_successor(cur_node.state):  # 开始expand
                    # 创建new_node
                    new_node = Node(next_tuple[0])
                    # new_node的path为cur_node的path加上新的move
                    new_path = copy.deepcopy(cur_node.path)
                    new_path.append(next_tuple[1])
                    new_node.path = new_path

                    # ucs
                    if calc_heuristic is not None and heuristic is None:
                        # ucs_compute_node_cost 计算new_node的cost
                        new_node.priority = calc_heuristic(next_tuple, cur_node)
                    # A*
                    if calc_heuristic is not None and heuristic is not None:
                        new_node.priority = calc_heuristic(problem, next_tuple, cur_node,
                                                           heuristic(problem, next_tuple[0]))

                    # 将new_node push进fringe
                    if isinstance(fringe, util.Stack) or isinstance(fringe, util.Queue):  # BFS/DFS
                        fringe.push(new_node)
                    elif isinstance(fringe, util.PriorityQueue):  # minimum cost (ucs search)
                        fringe.push(new_node, new_node.priority)
            else:
                continue
        else:
            path = cur_node.path
            return path, step
        "*** END YOUR CODE HERE ***"
    return []  # no path is found


def a_start_heuristic(problem, current_state):
    h = 0
    "*** YOUR CODE HERE ***"
    # 这里用了Manhattan distance和misplaced数量两种方法 打开注释以引用方法

    # Manhattan distance
    manhattan_i = []
    for i in range(N * N - 1):
        manhattan_i.append(
            sum(
                sum(
                    # np.argwhere 用于返回数组中等于i+1的索引值
                    abs(np.argwhere(np.array(problem.goal_state.cells).reshape(N, N) == i + 1)
                        - np.argwhere(np.array(current_state.cells).reshape(N, N) == i + 1))
                )
            )
        )
    h = sum(manhattan_i)

    # Number of title misplaced
    # misplaced = 0
    # for i in range(2):
    #     for j in range(2):
    #         if problem.goal_state.cells[i][j] != current_state.cells[i][j]:
    #             misplaced += 1
    # h = misplaced
    "*** END YOUR CODE HERE ***"
    return h


def a_start_cost(problem, successor, node, heuristic):
    cost = 0
    "*** YOUR CODE HERE ***"
    # f(n) = g(n) + h(n)
    cost = node.priority + successor[2] + heuristic
    "*** END YOUR CODE HERE ***"
    return cost


def a_start_search(problem):
    path = []
    step = 0
    "*** YOUR CODE HERE ***"
    path, step = search(problem, util.PriorityQueue(), a_start_cost, a_start_heuristic)
    "*** END YOUR CODE HERE ***"
    return path, step


def ucs_compute_node_cost(successor, node):
    """
    Define the method to compute cost within unit cost search
    hint: successor = (next_state, action, cost).
    however the cost for current node should be accumulative
    problem and heuristic should not be used by this function
    """
    "*** YOUR CODE HERE ***"
    # 当前node的cost加上new_node的cost
    cost = node.priority + successor[2]
    "*** END YOUR CODE HERE ***"
    return cost


def uniform_cost_search(problem):
    """
    Search the solution with minimum cost.
    """
    return search(problem, util.PriorityQueue(), ucs_compute_node_cost)


def breadth_first_search(problem):
    """
    Search the shallowest nodes in the search tree first.
    hint: using util.Queue as the fringe
    """

    "*** YOUR CODE HERE ***"
    path, step = search(problem, util.Queue())
    "*** END YOUR CODE HERE ***"
    return path, step


def depth_first_search(problem):
    """
    Search the deepest nodes in the search tree first.

    Your search algorithm needs to return a list of actions that reaches the
    goal. Make sure to implement a graph search algorithm.

    To get started, you might want to try some of these simple commands to
    understand the search problem that is being passed in:

    print "Start:", problem.get_start_state()
    print "Is the start a goal?", problem.is_goal(problem.get_start_state())
    print "Start's successors:", problem.get_successors(problem.get_start_state())

    hint: using util.Stack as the fringe
    """
    path = []
    step = 0
    "*** YOUR CODE HERE ***"
    path, step = search(problem, util.Stack())

    "*** END YOUR CODE HERE ***"
    return path, step
